#include "Sample_AgentDebug.h"
#include "imgui.h"
#include "MeshLoaderObj.h"
#include "InputGeom.h"
#include "RecastDebugDraw.h"
#include "SDL.h"
#include "SDL_opengl.h"
#include "DetourDebugDraw.h"

Sample_AgentDebug::Sample_AgentDebug():m_drawMode(DRAWMODE_NAVMESH), m_tileSize(48)
{
	setTool(new AgentDebugTool);
	m_pNavHandle = new CNavMeshHandle();
}

Sample_AgentDebug::~Sample_AgentDebug()
{
	if (m_NavMeshCrowd != nullptr)
	{
		delete m_NavMeshCrowd;
		m_NavMeshCrowd = nullptr;
	}
	if (m_pNavHandle != nullptr)
	{
		delete m_pNavHandle;
		m_pNavHandle = nullptr;
	}

}
void Sample_AgentDebug::handleSettings()
{
	Sample::handleCommonSettings();

	imguiLabel("Tiling");
	imguiSlider("TileSize", &m_tileSize, 16.0f, 128.0f, 8.0f);
	imguiSeparator();

	imguiIndent();
	imguiIndent();

	if (imguiButton("Load"))
	{
		dtFreeNavMesh(m_navMesh);
		dtFreeNavMeshQuery(m_navQuery);
		if (m_geom)
		{
			const rcMeshLoaderObj* pMesh = m_geom->getMesh();
			if (pMesh)
			{
				std::string strFileName = pMesh->getFileName();
				std::string strGeomName = strFileName;
				string_replace(strFileName, ".obj", ".bin");
				
				string_replace(strGeomName, ".obj", ".txt");

				bool bsucc = m_pNavHandle->LoadFile(1, strGeomName, strFileName);
				if (bsucc)
				{
					tileStruct tile;
					if (m_pNavHandle->GetData(1, &m_navMesh, &m_navQuery, &m_geomData, tile))
					{
						m_tileCache = tile.pTile;
						initNavMesh();

					}
				}
			}
		}
		
	}

	imguiUnindent();
	imguiUnindent();

	imguiSeparator();
}

void Sample_AgentDebug::string_replace(std::string &strBig, const std::string &strsrc, const std::string &strdst)
{
	std::string::size_type pos = 0;
	std::string::size_type srclen = strsrc.size();
	std::string::size_type dstlen = strdst.size();

	while ((pos = strBig.find(strsrc, pos)) != std::string::npos)
	{
		strBig.replace(pos, srclen, strdst);
		pos += dstlen;
	}
}

void Sample_AgentDebug::handleTools()
{
	int type = !m_tool ? TOOL_NONE : m_tool->type();
	if (imguiCheck("Create Agent Debug", type == TOOL_DEBUG))
	{
		setTool(new AgentDebugTool);
	}
	imguiSeparatorLine();

	imguiIndent();

	if (m_tool)
		m_tool->handleMenu();

	imguiUnindent();
}

void Sample_AgentDebug::handleDebugMode()
{
	// Check which modes are valid.
	bool valid[MAX_DRAWMODE];
	for (int i = 0; i < MAX_DRAWMODE; ++i)
		valid[i] = false;

	if (m_geom)
	{
		valid[DRAWMODE_NAVMESH] = m_navMesh != 0;
		valid[DRAWMODE_NAVMESH_TRANS] = m_navMesh != 0;
		valid[DRAWMODE_NAVMESH_BVTREE] = m_navMesh != 0;
		valid[DRAWMODE_NAVMESH_NODES] = m_navQuery != 0;
		valid[DRAWMODE_NAVMESH_PORTALS] = m_navMesh != 0;
		valid[DRAWMODE_NAVMESH_INVIS] = m_navMesh != 0;
		valid[DRAWMODE_MESH] = true;
		valid[DRAWMODE_CACHE_BOUNDS] = true;
	}

	int unavail = 0;
	for (int i = 0; i < MAX_DRAWMODE; ++i)
		if (!valid[i]) unavail++;

	if (unavail == MAX_DRAWMODE)
		return;

	imguiLabel("Draw");
	if (imguiCheck("Input Mesh", m_drawMode == DRAWMODE_MESH, valid[DRAWMODE_MESH]))
		m_drawMode = DRAWMODE_MESH;
	if (imguiCheck("Navmesh", m_drawMode == DRAWMODE_NAVMESH, valid[DRAWMODE_NAVMESH]))
		m_drawMode = DRAWMODE_NAVMESH;
	if (imguiCheck("Navmesh Invis", m_drawMode == DRAWMODE_NAVMESH_INVIS, valid[DRAWMODE_NAVMESH_INVIS]))
		m_drawMode = DRAWMODE_NAVMESH_INVIS;
	if (imguiCheck("Navmesh Trans", m_drawMode == DRAWMODE_NAVMESH_TRANS, valid[DRAWMODE_NAVMESH_TRANS]))
		m_drawMode = DRAWMODE_NAVMESH_TRANS;
	if (imguiCheck("Navmesh BVTree", m_drawMode == DRAWMODE_NAVMESH_BVTREE, valid[DRAWMODE_NAVMESH_BVTREE]))
		m_drawMode = DRAWMODE_NAVMESH_BVTREE;
	if (imguiCheck("Navmesh Nodes", m_drawMode == DRAWMODE_NAVMESH_NODES, valid[DRAWMODE_NAVMESH_NODES]))
		m_drawMode = DRAWMODE_NAVMESH_NODES;
	if (imguiCheck("Navmesh Portals", m_drawMode == DRAWMODE_NAVMESH_PORTALS, valid[DRAWMODE_NAVMESH_PORTALS]))
		m_drawMode = DRAWMODE_NAVMESH_PORTALS;
	if (imguiCheck("Cache Bounds", m_drawMode == DRAWMODE_CACHE_BOUNDS, valid[DRAWMODE_CACHE_BOUNDS]))
		m_drawMode = DRAWMODE_CACHE_BOUNDS;

	if (unavail)
	{
		imguiValue("Tick 'Keep Itermediate Results'");
		imguiValue("rebuild some tiles to see");
		imguiValue("more debug mode options.");
	}
}


void Sample_AgentDebug::drawTiles(duDebugDraw* dd, dtTileCache* tc)
{
	unsigned int fcol[6];
	float bmin[3], bmax[3];

	for (int i = 0; i < tc->getTileCount(); ++i)
	{
		const dtCompressedTile* tile = tc->getTile(i);
		if (!tile->header) continue;

		tc->calcTightTileBounds(tile->header, bmin, bmax);

		const unsigned int col = duIntToCol(i, 64);
		duCalcBoxColors(fcol, col, col);
		duDebugDrawBox(dd, bmin[0], bmin[1], bmin[2], bmax[0], bmax[1], bmax[2], fcol);
	}

	for (int i = 0; i < tc->getTileCount(); ++i)
	{
		const dtCompressedTile* tile = tc->getTile(i);
		if (!tile->header) continue;

		tc->calcTightTileBounds(tile->header, bmin, bmax);

		const unsigned int col = duIntToCol(i, 255);
		const float pad = tc->getParams()->cs * 0.1f;
		duDebugDrawBoxWire(dd, bmin[0] - pad, bmin[1] - pad, bmin[2] - pad,
			bmax[0] + pad, bmax[1] + pad, bmax[2] + pad, col, 2.0f);
	}

}

void Sample_AgentDebug::drawObstacles(duDebugDraw* dd, const dtTileCache* tc)
{
	// Draw obstacles
	for (int i = 0; i < tc->getObstacleCount(); ++i)
	{
		const dtTileCacheObstacle* ob = tc->getObstacle(i);
		if (ob->state == DT_OBSTACLE_EMPTY) continue;
		float bmin[3], bmax[3];
		tc->getObstacleBounds(ob, bmin, bmax);

		unsigned int col = 0;
		if (ob->state == DT_OBSTACLE_PROCESSING)
			col = duRGBA(255, 255, 0, 128);
		else if (ob->state == DT_OBSTACLE_PROCESSED)
			col = duRGBA(255, 192, 0, 192);
		else if (ob->state == DT_OBSTACLE_REMOVING)
			col = duRGBA(220, 0, 0, 128);

		duDebugDrawCylinder(dd, bmin[0], bmin[1], bmin[2], bmax[0], bmax[1], bmax[2], col);
		duDebugDrawCylinderWire(dd, bmin[0], bmin[1], bmin[2], bmax[0], bmax[1], bmax[2], duDarkenCol(col), 2);
	}
}

void Sample_AgentDebug::handleRender()
{
	if (!m_geom || !m_geom->getMesh())
		return;

	DebugDrawGL dd;

	const float texScale = 1.0f / (m_cellSize * 10.0f);

	// Draw mesh
	if (m_drawMode != DRAWMODE_NAVMESH_TRANS)
	{
		// Draw mesh
		duDebugDrawTriMeshSlope(&dd, m_geom->getMesh()->getVerts(), m_geom->getMesh()->getVertCount(),
			m_geom->getMesh()->getTris(), m_geom->getMesh()->getNormals(), m_geom->getMesh()->getTriCount(),
			m_agentMaxSlope, texScale);
		m_geom->drawOffMeshConnections(&dd);
	}

	if (m_tileCache && m_drawMode == DRAWMODE_CACHE_BOUNDS)
		drawTiles(&dd, m_tileCache);

	if (m_tileCache)
		drawObstacles(&dd, m_tileCache);


	glDepthMask(GL_FALSE);

	// Draw bounds
	const float* bmin = m_geom->getMeshBoundsMin();
	const float* bmax = m_geom->getMeshBoundsMax();
	duDebugDrawBoxWire(&dd, bmin[0], bmin[1], bmin[2], bmax[0], bmax[1], bmax[2], duRGBA(255, 255, 255, 128), 1.0f);

	// Tiling grid.
	int gw = 0, gh = 0;
	rcCalcGridSize(bmin, bmax, m_cellSize, &gw, &gh);
	const int tw = (gw + (int)m_tileSize - 1) / (int)m_tileSize;
	const int th = (gh + (int)m_tileSize - 1) / (int)m_tileSize;
	const float s = m_tileSize*m_cellSize;
	duDebugDrawGridXZ(&dd, bmin[0], bmin[1], bmin[2], tw, th, s, duRGBA(0, 0, 0, 64), 1.0f);

	if (m_navMesh && m_navQuery &&
		(m_drawMode == DRAWMODE_NAVMESH ||
			m_drawMode == DRAWMODE_NAVMESH_TRANS ||
			m_drawMode == DRAWMODE_NAVMESH_BVTREE ||
			m_drawMode == DRAWMODE_NAVMESH_NODES ||
			m_drawMode == DRAWMODE_NAVMESH_PORTALS ||
			m_drawMode == DRAWMODE_NAVMESH_INVIS))
	{
		if (m_drawMode != DRAWMODE_NAVMESH_INVIS)
			duDebugDrawNavMeshWithClosedList(&dd, *m_navMesh, *m_navQuery, m_navMeshDrawFlags/*|DU_DRAWNAVMESH_COLOR_TILES*/);
		if (m_drawMode == DRAWMODE_NAVMESH_BVTREE)
			duDebugDrawNavMeshBVTree(&dd, *m_navMesh);
		if (m_drawMode == DRAWMODE_NAVMESH_PORTALS)
			duDebugDrawNavMeshPortals(&dd, *m_navMesh);
		if (m_drawMode == DRAWMODE_NAVMESH_NODES)
			duDebugDrawNavMeshNodes(&dd, *m_navQuery);
		duDebugDrawNavMeshPolysWithFlags(&dd, *m_navMesh, SAMPLE_POLYFLAGS_DISABLED, duRGBA(0, 0, 0, 128));
	}


	glDepthMask(GL_TRUE);

	m_geom->drawConvexVolumes(&dd);

	if (m_tool)
		m_tool->handleRender();
	renderToolStates();
	if (m_NavMeshCrowd != nullptr)
		HandleAgentRender(dd);
	glDepthMask(GL_TRUE);

}

void Sample_AgentDebug::HandleAgentRender(DebugDrawGL &dd)
{
	const float rad = 0.6f;
	dd.depthMask(false);
	if (m_targetRef)
		duDebugDrawCross(&dd, m_targetPos[0], m_targetPos[1] + 0.1f, m_targetPos[2], rad, duRGBA(255, 255, 255, 192), 2.0f);
	// Trail
	for (int i = 0; i < m_NavMeshCrowd->getAgentCount(); ++i)
	{
		const dtCrowdAgent* ag = m_NavMeshCrowd->getAgent(i);
		if (!ag->active) continue;

		const AgentTrail* trail = &m_trails[i];
		const float* pos = ag->npos;

		dd.begin(DU_DRAW_LINES, 3.0f);
		float prev[3], preva = 1;
		dtVcopy(prev, pos);
		for (int j = 0; j < AGENT_MAX_TRAIL - 1; ++j)
		{
			const int idx = (trail->htrail + AGENT_MAX_TRAIL - j) % AGENT_MAX_TRAIL;
			const float* v = &trail->trail[idx * 3];
			float a = 1 - j / (float)AGENT_MAX_TRAIL;
			dd.vertex(prev[0], prev[1] + 0.1f, prev[2], duRGBA(0, 0, 0, (int)(128 * preva)));
			dd.vertex(v[0], v[1] + 0.1f, v[2], duRGBA(0, 0, 0, (int)(128 * a)));
			preva = a;
			dtVcopy(prev, v);
		}
		dd.end();
	}

	// Agent cylinders.
	for (int i = 0; i < m_NavMeshCrowd->getAgentCount(); ++i)
	{
		const dtCrowdAgent* ag = m_NavMeshCrowd->getAgent(i);
		//if (!ag->active) continue;

		const float radius = ag->params.radius;
		const float* pos = ag->npos;

		unsigned int col = duRGBA(0, 0, 0, 32);
		duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, col, 2.0f);
	}

	for (int i = 0; i < m_NavMeshCrowd->getAgentCount(); ++i)
	{
		const dtCrowdAgent* ag = m_NavMeshCrowd->getAgent(i);
		//if (!ag->active) continue;

		const float height = ag->params.height;
		const float radius = ag->params.radius;
		const float* pos = ag->npos;

		unsigned int col = duRGBA(220, 220, 220, 128);
		if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
			col = duLerpCol(col, duRGBA(128, 0, 255, 128), 32);
		else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
			col = duLerpCol(col, duRGBA(128, 0, 255, 128), 128);
		else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
			col = duRGBA(255, 32, 16, 128);
		else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
			col = duLerpCol(col, duRGBA(64, 255, 0, 128), 128);

		duDebugDrawCylinder(&dd, pos[0] - radius, pos[1] + radius*0.1f, pos[2] - radius,
			pos[0] + radius, pos[1] + height, pos[2] + radius, col);
	}


	// Velocity stuff.
	for (int i = 0; i < m_NavMeshCrowd->getAgentCount(); ++i)
	{
		const dtCrowdAgent* ag = m_NavMeshCrowd->getAgent(i);
		//if (!ag->active) continue;

		const float radius = ag->params.radius;
		const float height = ag->params.height;
		const float* pos = ag->npos;
		const float* vel = ag->vel;
		const float* dvel = ag->dvel;

		unsigned int col = duRGBA(220, 220, 220, 192);
		if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
			col = duLerpCol(col, duRGBA(128, 0, 255, 192), 32);
		else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
			col = duLerpCol(col, duRGBA(128, 0, 255, 192), 128);
		else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
			col = duRGBA(255, 32, 16, 192);
		else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
			col = duLerpCol(col, duRGBA(64, 255, 0, 192), 128);

		duDebugDrawCircle(&dd, pos[0], pos[1] + height, pos[2], radius, col, 2.0f);

		duDebugDrawArrow(&dd, pos[0], pos[1] + height, pos[2],
			pos[0] + dvel[0], pos[1] + height + dvel[1], pos[2] + dvel[2],
			0.0f, 0.4f, duRGBA(0, 192, 255, 192), 1.0f);

		duDebugDrawArrow(&dd, pos[0], pos[1] + height, pos[2],
			pos[0] + vel[0], pos[1] + height + vel[1], pos[2] + vel[2],
			0.0f, 0.4f, duRGBA(0, 0, 0, 160), 2.0f);
	}

	dd.depthMask(true);
}

void Sample_AgentDebug::handleRenderOverlay(double* proj, double* model, int* view)
{
	if (m_tool)
		m_tool->handleRenderOverlay(proj, model, view);
	renderOverlayToolStates(proj, model, view);
}

void Sample_AgentDebug::handleMeshChanged(class InputGeom* geom)
{
	Sample::handleMeshChanged(geom);
	if (m_tileCache != nullptr)
	{
		dtFreeTileCache(m_tileCache);
		m_tileCache = 0;
	}
	
	if (m_navMesh != nullptr)
	{
		dtFreeNavMesh(m_navMesh);
		m_navMesh = 0;
	}

	if (m_navQuery != nullptr)
	{
		dtFreeNavMeshQuery(m_navQuery);
		m_navQuery = 0;
	}
	

	if (m_tool)
	{
		m_tool->reset();
		m_tool->init(this);
	}
	resetToolStates();
	initToolStates(this);
}

bool Sample_AgentDebug::handleBuild()
{
	return true;
}

void Sample_AgentDebug::handleUpdate(const float dt)
{
	Sample::handleUpdate(dt);

	if (!m_navMesh)
		return;
	if (!m_tileCache)
		return;
	if ((m_NavMeshCrowd != nullptr)&&(m_bUpdate))
	{
		m_NavMeshCrowd->update(dt);
		// Update agent trails
		for (int i = 0; i < m_NavMeshCrowd->getAgentCount(); ++i)
		{
			const dtCrowdAgent* ag = m_NavMeshCrowd->getAgent(i);
			AgentTrail* trail = &m_trails[i];
			if (!ag->active)
				continue;
			// Update agent movement trail.
			trail->htrail = (trail->htrail + 1) % AGENT_MAX_TRAIL;
			dtVcopy(&trail->trail[trail->htrail * 3], ag->npos);
		}
	}
}

void Sample_AgentDebug::initNavMesh()
{
	if (m_NavMeshCrowd == nullptr)
		m_NavMeshCrowd = new CNavmeshCrowd();
	if ((m_navMesh != nullptr) && (m_navQuery != nullptr))
	{
		m_NavMeshCrowd->init(0.6f, m_navMesh, m_navQuery);
		// Make polygons with 'disabled' flag invalid.
		m_NavMeshCrowd->getEditableFilter(0)->setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED);
		dtObstacleAvoidanceParams params;
		memcpy(&params, m_NavMeshCrowd->getObstacleAvoidanceParams(0), sizeof(dtObstacleAvoidanceParams));
		// Low (11)
		params.velBias = 0.5f;
		params.adaptiveDivs = 5;
		params.adaptiveRings = 2;
		params.adaptiveDepth = 1;
		m_NavMeshCrowd->setObstacleAvoidanceParams(0, &params);
		// Medium (22)
		params.velBias = 0.5f;
		params.adaptiveDivs = 5;
		params.adaptiveRings = 2;
		params.adaptiveDepth = 2;
		m_NavMeshCrowd->setObstacleAvoidanceParams(1, &params);
		// Good (45)
		params.velBias = 0.5f;
		params.adaptiveDivs = 7;
		params.adaptiveRings = 2;
		params.adaptiveDepth = 3;
		m_NavMeshCrowd->setObstacleAvoidanceParams(2, &params);
		// High (66)
		params.velBias = 0.5f;
		params.adaptiveDivs = 7;
		params.adaptiveRings = 3;
		params.adaptiveDepth = 3;
		m_NavMeshCrowd->setObstacleAvoidanceParams(3, &params);
		TestAddAgent();
	}
	
}

void Sample_AgentDebug::TestAddAgent()
{
	if (m_NavMeshCrowd != nullptr)
	{
		CMonster *pM = new CMonster();
		MONSTER_INFO info;
		memset(&info, 0, sizeof(MONSTER_INFO));
		info.idx = 0;
		info.radius = 0.6f;
		info.height = 2.0f;
		info.maxAcceleration = 8.0f;
		info.maxSpeed = 3.5f;
		info.updateFlags = DT_CROWD_ANTICIPATE_TURNS | DT_CROWD_OPTIMIZE_VIS | DT_CROWD_OPTIMIZE_TOPO | DT_CROWD_OBSTACLE_AVOIDANCE;
		info.obstacleAvoidanceType = 0;
		info.separationWeight = 3.0f;
		info.queryFilterType = 0;
		info.modelId = 5;
		info.bObstacle = false;
		info.bCollisions = true;
		float origion[3] = { 20.64543f, 0.0f, -107.5341f };
		info.bornPos.x = origion[0];
		info.bornPos.y = origion[1];
		info.bornPos.z = origion[2];
		pM->Init(info);
		bool bsuccAdd = m_NavMeshCrowd->addAgent(pM);
		if (bsuccAdd)
		{
			AgentTrail* trail = &m_trails[info.idx];
			for (int i = 0; i < AGENT_MAX_TRAIL; ++i)
				dtVcopy(&trail->trail[i * 3], origion);
			trail->htrail = 0;
			TestSetTarget(info.idx);
		}
	}
	
}

void Sample_AgentDebug::TestSetTarget(int idx)
{
	float targetPos[3] = { 46.19549f, 0.0f, -168.7159f };
	m_targetRef = 0;
	float nearestPos[3] = { 0, 0, 0 };
	const dtQueryFilter* filter = m_NavMeshCrowd->getFilter(0);
	const float* ext = m_NavMeshCrowd->getQueryExtents();
	m_navQuery->findNearestPoly(targetPos, ext, filter, &m_targetRef, nearestPos);
	bool bsuccTarget = m_NavMeshCrowd->SetMoveTarget(idx, m_targetRef, nearestPos);
	dtVcopy(m_targetPos, nearestPos);
	
	
}